#include <stdio.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"


#define LED_TEST_GPIO 9 // for hispark_pegasus
#define IOT_GPIO_SERVO_1 2
#define IOT_GPIO_SERVO_2 4


/*舵机1*/
void servo_1_0(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,1);
        hi_udelay(1450);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,0);
        hi_udelay(19375-1450);
    }

    hi_udelay(5000);
    
    return NULL;

}

void servo_1_inv90(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,1);
        hi_udelay(500);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,0);
        hi_udelay(19375-500);
    }

    hi_udelay(5000);
    
    return NULL;

}

void servo_1_90(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,1);
        hi_udelay(2425);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,0);
        hi_udelay(19375-2425);
    }

    hi_udelay(500000);
    
    return NULL;

}

void servo_1_inv45(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,1);
        hi_udelay(965);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,0);
        hi_udelay(19375-965);
    }

    hi_udelay(5000);
    
    return NULL;

}

void servo_1_45(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,1);
        hi_udelay(1950);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,0);
        hi_udelay(19375-1950);
    }

    hi_udelay(5000);
    
    return NULL;

}


/*舵机2*/
void servo_2_0(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,1);
        hi_udelay(1450);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,0);
        hi_udelay(19375-1450);
    }

    hi_udelay(5000);
    
    return NULL;

}

void servo_2_inv90(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,1);
        hi_udelay(500);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,0);
        hi_udelay(19375-500);
    }

    hi_udelay(5000);
    
    return NULL;

}

void servo_2_90(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,1);
        hi_udelay(2425);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,0);
        hi_udelay(19375-2425);
    }

    hi_udelay(5000);
    
    return NULL;

}

void servo_2_inv45(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(50000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(50000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,1);
        hi_udelay(965);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,0);
        hi_udelay(19375-965);
    }

    hi_udelay(5000);
    
    return NULL;

}

void servo_2_45(void)
{
    
    /*闪灯*/
    for(int i=0;i<2;i++){
        IoTGpioSetOutputVal(LED_TEST_GPIO,1);
        hi_udelay(100000);
        IoTGpioSetOutputVal(LED_TEST_GPIO,0);
        hi_udelay(200000);
    }
    
    /*1500*/
    for(int i=0;i<9;i++){
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,1);
        hi_udelay(1950);                                  
        IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,0);
        hi_udelay(19375-1950);
    }

    hi_udelay(5000);
    
    return NULL;

}

static void *servoTask(const char *arg)
{
    
    IoTGpioInit(LED_TEST_GPIO);
    IoSetFunc(LED_TEST_GPIO,0);
    IoTGpioSetDir(LED_TEST_GPIO,IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(LED_TEST_GPIO,0);
    
    IoTGpioInit(IOT_GPIO_SERVO_1);
    IoSetFunc(IOT_GPIO_SERVO_1,0);
    IoTGpioSetDir(IOT_GPIO_SERVO_1,IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(IOT_GPIO_SERVO_1,0);

    IoTGpioInit(IOT_GPIO_SERVO_2);
    IoSetFunc(IOT_GPIO_SERVO_2,0);
    IoTGpioSetDir(IOT_GPIO_SERVO_2,IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(IOT_GPIO_SERVO_2,0);
    
    (void)arg;
    
    servo_Task_right();
    hi_udelay(2000000);
    servo_Task_left();
    
   
    printf("servo work");
    return NULL;

}

/*static void servoExampleEntry(void)
{
    osThreadAttr_t attr;

    


    attr.name = "servoTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 1024;
    attr.priority = 25;

    if (osThreadNew((osThreadFunc_t)servoTask, NULL, &attr) == NULL) {
        printf("[LedExample] Falied to create LedTask!\n");
    }
}

SYS_RUN(servoExampleEntry);*/